#include "Scene.h"

#include <QGraphicsItem>
#include <QPixmap>
#include <QPainter>
#include <QPaintEngine>
#include <QtOpenGL>

#include <AR/ar.h>
#include <AR/video.h>
#include <AR/config.h>
#include <AR/param.h>
#include <AR/gsub_lite.h>
#include <lib3ds.h>
#include <coldet.h>
#include <QSound>


using namespace cv;
using namespace std;

static double matrix[16] = {0.0f};
static float fmatrix[16] = {0.0f};
double leftOne = 0.01;
double rightOne = 0.01;
GLUquadricObj *quadric = gluNewQuadric();
int wid = 640;
int hei = 480;
object3ds *model1;
object3ds *model2;
CollisionModel3D *m1;
CollisionModel3D *m2;
int counting = 0;


/**********************
 * Constructor
 */
CScene::CScene(QObject *parent) : QGraphicsScene(parent)
{
    StartCapture(1280, 960);
    m_NyART = new ARToolkitAPI();
    m_NyART->ARInit(640, 480);
    gluQuadricDrawStyle(quadric, GLU_FILL);
    gluQuadricNormals(quadric, GLU_SMOOTH);
    gluQuadricOrientation(quadric, GLU_OUTSIDE);

    // load 3ds models
    model1 = new object3ds;
    model2 = new object3ds;
    loadModel("include/penguin.3ds", model1);
    loadModel("include/penguin.3ds", model2);

    //load collision detection models
    m1 = newCollisionModel3D(true);
    m2 = newCollisionModel3D(true);
    forCollision (m1, model1, leftOne);
    forCollision (m2, model2, rightOne);
}
// end CSene();


/**********************
 * Destructor
 */
CScene::~CScene()
{
    m1->~CollisionModel3D();
    m2->~CollisionModel3D();
    if(model1->model != NULL){
        lib3ds_file_free(model1->model);
    }
    if(model2->model != NULL){
        lib3ds_file_free(model2->model);
    }
    free(model1);
    free(model2);
    StopCapture();
}
// end ~CScene()


/**********************
 * Counts the total faces of the model
 */
void CScene::getfaces(object3ds *todraw)
{
    todraw->total_face=0;
    for(long meshcount=0;meshcount<todraw->model->nmeshes;meshcount++){
        todraw->total_face+=todraw->mesh[meshcount]->nfaces;
    }
}
// end getFaces()


/**********************
 * Loads the elements of a 3DS model
 */
void CScene::loadModel(char *name, object3ds *modelling){
    modelling->model = lib3ds_file_open(name);
    if(!modelling->model){
        qDebug("Model %f did not load", name);
    } else {
        modelling->mesh = modelling->model->meshes;
        modelling->material= modelling->model->materials;
        modelling->camera= modelling->model->cameras;
        modelling->light= modelling->model->lights;
        getfaces(modelling);
    }
}
// end loadModel()


/**********************
 * Updates the values from the dynamometers - 0% ~ 100% and over
 */
void CScene::ChangeChildValues(double l, double r){
    leftOne = l/100;
    rightOne = r/100;
    if(leftOne <= 0) leftOne = 0.01;
    if(rightOne <= 0) rightOne = 0.01;
}
// end ChangeChildvalues()


/**********************
 * Start video feed
 */
bool CScene::StartCapture(int width, int height)
{
    // Start capture
    m_OpenCV.StartCapture(width, height);

    QTimer::singleShot(50, this, SLOT(update()));

    return true;
}
// end StartCapture()


/**********************
 * Stop video feed
 */
bool CScene::StopCapture()
{
    m_OpenCV.StopCapture();
    return true;
}
// end StopCapture()


/**********************
 * Controls lighting of the scene
 */
void lightup()
{
    glShadeModel(GL_SMOOTH);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);
    glFrontFace(GL_CCW);
    float gambient[]={0.2,0.2,0.2,1.0};
    float lightpos[]={1.0,1.0,1.0,0.0};
    float lambient[]={1.0,1.0,1.0,1.0};
    float ldiffuse[]={0.8,0.8,0.8,1.0};
    float lspecular[]={0.3,0.3,0.3,1.0};
    glLightfv(GL_LIGHT0,GL_DIFFUSE,ldiffuse);
    glLightfv(GL_LIGHT0,GL_AMBIENT,lambient);
    glLightfv(GL_LIGHT0,GL_SPECULAR,lspecular);
    glLightfv(GL_LIGHT0,GL_POSITION,lightpos);
    glLightModelfv(GL_LIGHT_MODEL_AMBIENT,gambient);
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
}
// end lightup()


/**********************
 * Controls all rendering of the scene
 */
void CScene::drawBackground(QPainter *painter, const QRectF &rect)
{
    // Check if current painter is a OpenGL painter
    if((painter->paintEngine()->type() != QPaintEngine::OpenGL2) &&
            (painter->paintEngine()->type() != QPaintEngine::OpenGL))
    {
        return;
    }

    // every 5 frames, update the scale of the model for collision detection
    if(counting++ == 5){

        m1->~CollisionModel3D(); //detroy model
        m1 = newCollisionModel3D(true); //make new one
        forCollision (m1, model1, leftOne); // update model to scale determined by dynamometer

        m2->~CollisionModel3D();
        m2 = newCollisionModel3D(true);
        forCollision (m2, model2, rightOne);

        counting = 0;
    }

    // Set clear color to white
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    // Set clear depth bit
    glClearDepth(1.0f);
    // Clear screen
    glDepthFunc(GL_LEQUAL);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // Grab image from the webcam
    IplImage *cvimage = m_OpenCV.QueryFrame();

    // resize the picture to fill the screen. Used for window resize.
    if(cvimage != NULL)
    {
        IplImage *cvimage2 = cvCreateImage(cvSize(rect.width(), rect.height()), cvimage->depth, cvimage->nChannels);
        cvResize(cvimage, cvimage2, CV_INTER_LINEAR);
        glDrawPixels(cvimage2->width, cvimage2->height, GL_RGB, GL_UNSIGNED_BYTE, cvimage2->imageData);
        cvReleaseImage(&cvimage2);
        cvimage2 = NULL;
    }

    //glDepthFunc(GL_LEQUAL);
    lightup();
    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);

    bool isMarkerDetected = false;

    if(m_OpenCV.GetBGRAImage() != NULL)
    {
        // Is marker presence on the image?
        isMarkerDetected = m_NyART->FindShit(m_OpenCV.GetImage());

        if(isMarkerDetected)
        {

            glMatrixMode(GL_PROJECTION);
            glLoadIdentity();
            gluPerspective(45.0f, static_cast<float>(640) / static_cast<float>(480), 0.1f, 1000.0f);

            glMatrixMode(GL_MODELVIEW);
            glLoadIdentity();

            // for model one
            glLoadMatrixd(m_NyART->GetTransformationMatrix(matrix, 0));
            glLoadMatrixd(matrix);
            DrawOtherThings(model1, leftOne);
            for(int e = 0; e < 16; e++){
                fmatrix[e] = matrix[e];
            }
            m1->setTransform(fmatrix);

            // for model two
            glLoadMatrixd(m_NyART->GetTransformationMatrix(matrix, 1));
            glLoadMatrixd(matrix);
            DrawOtherThings(model2, rightOne);
            for(int e = 0; e < 16; e++){
                fmatrix[e] = matrix[e];
            }
            m2->setTransform(fmatrix);

            //check for collision now
            checkCollisions();

        }
    }
    // Timer will update our scene in 50 milliseconds
    QTimer::singleShot(50, this, SLOT(update()));
}
// end drawBackground()


/**********************
 * Checks for collisions, and handles them
 */
void CScene::checkCollisions(){
    if(m1->collision(m2)){
        qDebug() << "hit!!!";
    } else {
        qDebug() << "not hit!!!";
    }
}
// end checkCollisions()


/**********************
 * Draws the model to scale
 */
void CScene::DrawOtherThings(object3ds *todraw, float modelScale)
{
    glScalef(modelScale, modelScale, modelScale);
    long meshcount;
    for(meshcount=0;meshcount<todraw->model->nmeshes;meshcount++)
    {
        todraw->face=todraw->mesh[meshcount]->faces;
        for(long i=0;i<todraw->mesh[meshcount]->nfaces;i++)
        {
            glMaterialfv(GL_FRONT,GL_DIFFUSE,todraw->material[todraw->face[i].material]->diffuse);
            glMaterialfv(GL_FRONT,GL_AMBIENT,todraw->material[todraw->face[i].material]->ambient);
            glMaterialfv(GL_FRONT,GL_SPECULAR,todraw->material[todraw->face[i].material]->specular);
            glBegin(GL_TRIANGLES);
            glVertex3fv(todraw->mesh[meshcount]->vertices[todraw->face[i].index[0]]);
            glVertex3fv(todraw->mesh[meshcount]->vertices[todraw->face[i].index[1]]);
            glVertex3fv(todraw->mesh[meshcount]->vertices[todraw->face[i].index[2]]);
            glEnd();
        }
    }
}
// end DrawOtherThings()


/**********************
 * Scales a vector
 */
void scaleIt(float *toscale, float scalex, float scaley, float scalez){
    toscale[0] = toscale[0]*scalex;
    toscale[1] = toscale[1]*scaley;
    toscale[2] = toscale[2]*scalez;
} // end scaleIt()


/**********************
 * Copies elements of a float array representing a vector
 */
void copyArray(float *source, float *destination){
    for(int i = 0; i < 3; i++){
        destination[i] = source[i];
    }
}
// end copyArray()


/**********************
 * Loads all triangles from the model, and scales it
 */
void CScene::forCollision(CollisionModel3D *toaddto, object3ds *todraw, float scaleFactor)
{
    long meshcount;
    for(meshcount=0;meshcount<todraw->model->nmeshes;meshcount++)
    {
        todraw->face=todraw->mesh[meshcount]->faces;
        for(long i=0;i<todraw->mesh[meshcount]->nfaces;i++)
        {
            float a[3];
            float b[3];
            float c[3];
            copyArray(todraw->mesh[meshcount]->vertices[todraw->face[i].index[0]], a);
            copyArray(todraw->mesh[meshcount]->vertices[todraw->face[i].index[1]], b);
            copyArray(todraw->mesh[meshcount]->vertices[todraw->face[i].index[2]], c);
            scaleIt(a, scaleFactor,scaleFactor,scaleFactor);
            scaleIt(b, scaleFactor,scaleFactor,scaleFactor);
            scaleIt(c, scaleFactor,scaleFactor,scaleFactor);
            toaddto->addTriangle(a, b, c);
        }
    }
    toaddto->finalize();
}
// end forCollision()

